package ncsutest.robowulf;

import com.sun.spot.resourcesharing.ResourceUnavailableException;
import com.sun.spot.sensorboard.io.IScalarInput;
import com.sun.spot.sensorboard.peripheral.ITriColorLED;
import com.sun.spot.sensorboard.peripheral.LEDColor;
import com.sun.spot.util.Utils;

import ncsutest.NCSUCalibrationBase;
import ncsutest.robowulf.sensors.IRSensor;
import ncsutest.util.LEDs;
import ncsutest.util.Switches;

public class RobowulfCalibration extends NCSUCalibrationBase {
	private static final int IR_SENSOR_LOW = IRSensor.WHITE_THRESHOLD; 
	private static final int IR_SENSOR_HIGH = IRSensor.BLACK_THRESHOLD; 
	
	public RobowulfCalibration(int[] calibs) {
		super(calibs);
	}

	/**
	 * performCalibration00: Calibrate the left IR sensor range
	 */
	public void performCalibration00() throws Exception {
		calibrateIRSensor(RobowulfTests.irSensorPins[0]);
	}

	/**
	 * performCalibration01: Calibrate the right IR sensor range
	 */
	public void performCalibration01() throws Exception {
		calibrateIRSensor(RobowulfTests.irSensorPins[1]);
	}

	
	
	private void calibrateIRSensor(IScalarInput pin) throws Exception {
		Thread waitThread = new Thread() {
			public void run() {
				while (Switches.getSwitches().waitForSwitchRelease() != Switches.BOTH);
			}
		};
		waitThread.start();
		
		IRSensor leftSensor = new IRSensor("Left", pin);
		
		int val;
		while (waitThread.isAlive()) {
			val = (int)leftSensor.getRawValue();
			System.out.println(val);
			
			int i = 0;
			
			if (val < IR_SENSOR_LOW)
				for (; i < 1; i++) LEDs.setLED(i, LEDColor.RED);
			else if (val > IR_SENSOR_HIGH)
				for (; i < 3; i++) LEDs.setLED(i, LEDColor.GREEN);
			else
				for (; i < 2; i++) LEDs.setLED(i, LEDColor.YELLOW);

			for (; i < 3; i++) LEDs.clearLED(i);
			
			Utils.sleep(50);
		}
		
		leftSensor.destroy();
	}
}
